/*
 * test_robots.cpp
 *
 *  Created on: Mar 19, 2009
 *      Author: iveb
 */
#include <iostream>
#include "robot.h"
#include "waypoint.h"
#define MAX 5
using namespace std;

int main()
{
	Robot robots[MAX];
    cout<< "Testing robot.cpp"<<endl;

	int i=1;

	for(i=1;i<=MAX;i++)
	{
		robots[i].setId(i);
		cout << "Robot " << i << ": id = " << robots[i].getId() <<endl;
	}
	
	WayPoint w1;
	w1.setX(5.0);
	w1.setY(9.0);
	w1.setTimeStamp(1);
	robots[1].setWp(w1);
	
	WayPoint w2;
	w2.setX(1.0);
	w2.setY(1.0);
	w2.setTimeStamp(4);
	robots[1].setWp(w2);
	
	WayPoint wp_f = robots[1].getWpNext(6);
	cout << "Robot " << 1 << ": id = " << robots[1].getId() << ": estimated X = " << wp_f.getX() << ": estimated Y = " << wp_f.getY() <<endl;

    return 0;
}
